
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       Vibration.c
  * @author     baiyang
  * @date       2021-7-14
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "Vibration.h"

/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
//初始化参数配置
void VibrationMode_Init(VibrationMode* pVib, float weight, float rpm_max, float gain, float num)
{
    pVib->_weight = weight;
    pVib->_rpm_max = rpm_max;
    pVib->_gain = gain;

    pVib->num = num;
    pVib ->vel_integ = (float *)malloc(sizeof(float) * pVib->num);
    memset(pVib->vel_integ, 0, sizeof(float) * pVib->num);
    pVib->enable_vib = true;
}

void VibrationMode_Update(VibrationMode* pVib, Vector3f_t* pState_out, const Vector3f_t* pState_in, float* prop_vel, float dt)
{
    //计算螺旋桨震动
    Vector3f_t vib_value;    //三轴震动量
    vec3_zero(&vib_value);

    if (pVib->enable_vib)
    {
        for (uint8_t cnt = 0; cnt < pVib->num; cnt++)
        {
            pVib->vel_integ[cnt] += prop_vel[cnt] * dt;
            float sin_rot, cos_rot;

            sin_rot = sinf(pVib->vel_integ[cnt]);
            cos_rot = cosf(pVib->vel_integ[cnt]);

            float ratio;
            ratio = SQ(prop_vel[cnt] / (pVib->_rpm_max * RPM_TO_RPS));

            vib_value.x += ratio * sin_rot * pVib->_weight;
            vib_value.y += ratio * cos_rot * pVib->_weight;
        }

        vib_value.z = 0.5f * (vib_value.x + vib_value.y);
        vec3_mult(&vib_value, &vib_value, (pVib->_gain / 4.0f));
    }

    vec3_add(pState_out, pState_in, &vib_value);
}

void VibrationMode_Enable(VibrationMode* pVib)
{
    if (!pVib->enable_vib)
    {
        pVib->enable_vib = true;
    }
    return;
}

void VibrationMode_Disable(VibrationMode* pVib)
{
    if (pVib->enable_vib)
    {
        pVib->enable_vib = false;
    }
    return;
}

/*------------------------------------test------------------------------------*/


